
#include "../linalg/gsl_ext_linalg.h"
#include "../matrix/gsl_ext_matrix.h"
#include "gsl_ext_multimin.h"
#include "linmin.h"

#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>

typedef struct
{
  lineminimizer *line_minimizer;
  gsl_vector *d;
  gsl_vector *x_plus;
  double f_plus;
  gsl_vector *gradient_plus;
  double tol;
  gsl_matrix *tmpH;
  gsl_matrix *L;
  gsl_vector *e;
  gsl_matrix *muI;
}
mnewton_state_t;

int mnewton_alloc (void *vstate, size_t n)
{
  mnewton_state_t *state = (mnewton_state_t *) vstate;
  
  state->line_minimizer = lineminimizer_alloc(lineminimizer_morethuente, n);
  lineminimizer_morethuente_set_gamma(state->line_minimizer, 0.0);
  
  state->d = gsl_vector_alloc(n);
  state->x_plus = gsl_vector_alloc(n);
  state->gradient_plus = gsl_vector_alloc(n);
  state->tmpH = gsl_matrix_alloc(n, n);
  state->L = gsl_matrix_alloc(n, n);
  state->e = gsl_vector_alloc(n);
  state->muI = gsl_matrix_alloc(n, n);
  
  return GSL_SUCCESS;
}

int mnewton_restart (void *state)
{
  return GSL_SUCCESS;
}

void mnewton_free (void *vstate)
{
  mnewton_state_t *state = (mnewton_state_t *) vstate;
  
  gsl_matrix_free(state->muI);
  gsl_vector_free(state->e);
  gsl_matrix_free(state->L);
  gsl_matrix_free(state->tmpH);
  gsl_vector_free(state->gradient_plus);
  gsl_vector_free(state->x_plus);
  gsl_vector_free(state->d);
  
  lineminimizer_free(state->line_minimizer);
}

int mnewton_set (void *vstate, gsl_ext_multimin_function_fd2f * fd2f,
                 const gsl_vector *x, double *f,
                 gsl_vector * gradient, gsl_matrix *hessian,
                 double step_size, double tol)
{
  mnewton_state_t *state = (mnewton_state_t *) vstate;
  
  GSL_EXT_MULTIMIN_FN_EVAL_F_D2F (fd2f, x, f, gradient, hessian);
  state->tol = tol;
  
  return GSL_SUCCESS;
}

int mnewton_iterate (void *vstate,gsl_ext_multimin_function_fd2f * fd2f,
                     gsl_vector * x, double * f,
                     gsl_vector * gradient, gsl_matrix *hessian,
                     gsl_vector * dx)
{
  mnewton_state_t *state = (mnewton_state_t *) vstate;
  double alpha;
  double e_diag_max;
  double minev, maxev;
  double mu;
  
  /* evaluate the Hessian matrix */
  GSL_EXT_MULTIMIN_FN_EVAL_D2F(fd2f, x, hessian);
  
  /* solve the Newton equation Hd=-g by Cholesky decomposition */
  /* use the modified LDL^T decomposition to ensure */
  /* that the Hessian is positive definite */
  gsl_blas_dcopy(gradient, state->d);
  gsl_blas_dscal(-1.0, state->d);
  
  gsl_matrix_memcpy(state->tmpH, hessian);
  
  gsl_ext_linalg_modified_cholesky_decomp(hessian, state->L, state->e);
  e_diag_max = gsl_vector_max(state->e);
  gsl_ext_matrix_compute_gerschgorin_bounds(hessian, &minev, &maxev);
  mu = GSL_MIN_DBL(e_diag_max, GSL_MAX_DBL(-minev, 0.0));
  
  gsl_matrix_set_identity(state->muI);
  gsl_matrix_scale(state->muI, mu);
  gsl_matrix_add(state->tmpH, state->muI);
  
  /* solve the Newton equation */
  gsl_error_handler_t *oldHandler = gsl_set_error_handler_off();
  
  if(gsl_linalg_cholesky_decomp(state->tmpH) != GSL_EDOM)
    gsl_linalg_cholesky_svx(state->tmpH, state->d);
  
  gsl_set_error_handler(oldHandler);
  
  /* obtain the step length */
  lineminimizer_minimize(state->line_minimizer, (gsl_multimin_function_fdf *)fd2f,
                         1.0, state->tol, *f, x, gradient, state->d,
                         &alpha, state->x_plus, &state->f_plus, state->gradient_plus);
  
  /* update the current iterate and its function and gradient values */
  gsl_blas_dcopy(state->x_plus, dx);
  gsl_blas_daxpy(-1.0, x, dx);
  gsl_blas_dcopy(state->x_plus, x);
  gsl_blas_dcopy(state->gradient_plus, gradient);
  *f = state->f_plus;
  
  return GSL_SUCCESS;
}

static const gsl_ext_multimin_fd2fminimizer_type mnewton_type = 
  {
    "mnewton",
    sizeof (mnewton_state_t),
    &mnewton_alloc,
    &mnewton_set,
    &mnewton_iterate,
    &mnewton_restart,
    &mnewton_free
  };

const gsl_ext_multimin_fd2fminimizer_type
* gsl_ext_multimin_fd2fminimizer_mnewton = &mnewton_type;
